#pragma once

#include <utility>
#include <boost/unordered_map.hpp>
#include <iostream>
#include "binary_heap.h"
#include "graph_algorithms.h"

namespace details
{
  struct vertex_info
  {
    vertex_info(vertex_id v, heap_id h, float dist, vertex_id pred) :
      v_id(v), h_id(h), dist(dist), pred(pred)
    {
    }

    vertex_id v_id;
    heap_id h_id;
    float dist;
    vertex_id pred;
  };

  struct dijkstra_graph_adapter_t
  {
    dijkstra_graph_adapter_t(db_graph_t const & graph) :
      graph_(graph)
    {
    }

    std::pair<float, vertex_id> dequeue()
    {
      return heap_.extract_top();
    }

    void decrease_key(vertex_id v, float new_dist, vertex_id new_pred)
    {
      map_t::iterator it = info_map_.find(v);
      if (it == info_map_.end())
      {
        info_map_.insert(
                         std::make_pair(
                                        v,
                                        vertex_info(v, heap_.push(new_dist, v),
                                                    new_dist, new_pred)));
      }
      else
      {
        heap_.decrease_key(it->second.h_id, new_dist);
        it->second.dist = new_dist;
        it->second.pred = new_pred;
      }
    }

    bool empty() const
    {
      return heap_.empty();
    }

    vertex_id pred(vertex_id v) const
    {
      return info_map_.find(v)->second.pred;
    }

    float get_dist(vertex_id v) const
    {
      map_t::const_iterator it = info_map_.find(v);
      if (it == info_map_.end())
        return std::numeric_limits<float>::max();
      else
        return it->second.dist;
    }

    bool was_processed(vertex_id v) const
    {
      return (info_map_.find(v) != info_map_.end());
    }
  private:
    typedef boost::unordered_map<vertex_id, vertex_info> map_t;
    db_graph_t const & graph_;
    binary_heap_t<float, vertex_id> heap_;
    map_t info_map_;
  };
}

float dist(geodata_t g1, geodata_t g2)
{
  float dx = g1.lat - g2.lat;
  float dy = g1.lon - g2.lon;
  return sqrt(dx * dx + dy * dy);
}

float h(db_graph_t const & g, vertex_id v1, vertex_id v2)
{
  geodata_t v1_d = g.get_data(v1);
  geodata_t v2_d = g.get_data(v2);

  return dist(v1_d, v2_d) / 1.001;
}

// Writes edges of the shortest path to the out in reverse order
// Returns the length of the shortest path between v1 and v2 or FLT_MAX
//   if no such path exists
template<class OutIter>
float dijkstra_shortest_path(db_graph_t const & g, vertex_id v1, vertex_id v2,
                             OutIter out)
{
  details::dijkstra_graph_adapter_t a(g);

  a.decrease_key(v1, 0.f, v1);
  while (!a.empty())
  {
    std::pair<float, vertex_id> dist_vertex = a.dequeue();
    if (dist_vertex.second == v2)
      break;

    geodata_t dist_data = g.get_data(dist_vertex.second);
    adj_vertices_range_t adj = g.adjacent_range(dist_vertex.second);

    for (adj_vertices_range_t::iterator it = adj.begin(); it
        != adj.end(); ++it)
    {
      float new_dist = it->weight + dist_vertex.first + h(g, v1, dist_vertex.second) - h(g, v1, it->vertex_index);
      assert(new_dist >= 0.0);
      if (new_dist < a.get_dist(it->vertex_index))
      {
        a.decrease_key(it->vertex_index, new_dist, dist_vertex.second);
      }
    }
  }

  if (a.was_processed(v2))
  {
    vertex_id cur_v = v2;
    while (cur_v != v1)
    {
      *out = cur_v;
      cur_v = a.pred(cur_v);
    }
    *out = v1;
  }

  return a.get_dist(v2);
}
